/*!
\copyright  Copyright (c) 2024 Qualcomm Technologies International, Ltd.
            All Rights Reserved.
            Qualcomm Technologies International, Ltd. Confidential and Proprietary.
\file
*/

#include "dynamic_latency_adjustment.h"
#include "dynamic_latency_adjustment_config.h"
#include "dynamic_latency_adjustment_handover.h"
#include <latency_config.h>

#include <audio_driver_feature.h>
#include <audio_stack_parameters.h>
#include <audio_stack_feature_audiosource.h>

#include <logging.h>

#include <bt_device.h>
#include <mirror_profile.h>

#ifdef INCLUDE_HDMA_LINK_QUALITY_EVENT
#include <hdma_core.h>
#endif

typedef enum
{
    DYNAMIC_LATENCY_ADJUSTEMENT_INTERNAL_UPDATE_LATENCY,
} dynamic_latency_adjustemt_messages_t;

/*! \brief Dynamic latency state */
typedef struct
{
    /*! Current dynamic latency in milli-seconds */
    uint16 dynamic_latency;

    uint16 fake_lq;

    /*! This flag tells if Dynamic Latency Adjustment feature is enabled. This
        flag is disabled by default. Unless it is set latency will not be
        dynamically updated. */
    unsigned is_enabled : 1;

    /*! This flag is set when dynamic adjustment is currently active */
    unsigned active : 1;

} dynamic_latency_adjustment_ctx_t;

static dynamic_latency_adjustment_ctx_t ctx;

static void dynamicLatencyAdjustment_MessageHandler(Task task, MessageId id, Message message);
static bool dynamicLatencyAdjustment_CanStart(void);
static uint16 dynamicLatencyAdjustment_GetAdjustedLatency(void);
static void dynamicLatencyAdjustment_ApplyLatency(uint16 adjusted_latency);
static void dynamicLatencyAdjustment_UpdateLatency(void);

TaskData task_data = {.handler = dynamicLatencyAdjustment_MessageHandler};

bool DynamicLatencyAdjustment_Init(Task init_task)
{
    UNUSED(init_task);

    memset(&ctx, 0, sizeof(ctx));
    ctx.dynamic_latency = LatencyConfig_GetTwsLatency();
    MirrorProfile_ClientRegister((Task)&task_data);

    return TRUE;
}

bool DynamicLatencyAdjustment_IsEnabled(void)
{
    return ctx.is_enabled;
}

void DynamicLatencyAdjustment_Enable(void)
{
    DEBUG_LOG_VERBOSE("DynamicLatencyAdjustment_Enable");
    ctx.is_enabled = TRUE;
}

void DynamicLatencyAdjustment_Disable(void)
{
    DEBUG_LOG_VERBOSE("DynamicLatencyAdjustment_Disable");
    ctx.is_enabled = FALSE;
}

void DynamicLatencyAdjustment_Start(void)
{
    if(!ctx.active && dynamicLatencyAdjustment_CanStart())
    {
        DEBUG_LOG_VERBOSE("DynamicLatencyAdjustment_Start");
        
        ctx.active = TRUE;
        dynamicLatencyAdjustment_UpdateLatency();
    }
}

void DynamicLatencyAdjustment_Stop(void)
{
    if(ctx.active)
    {
        DEBUG_LOG_VERBOSE("DynamicLatencyAdjustment_Stop");
        
        ctx.active = FALSE;
        ctx.dynamic_latency = LatencyConfig_GetTwsLatency();
        MessageCancelAll((Task)&task_data, DYNAMIC_LATENCY_ADJUSTEMENT_INTERNAL_UPDATE_LATENCY);
    }
}

static void dynamicLatencyAdjustment_MessageHandler(Task task, MessageId id, Message message)
{
    DEBUG_LOG_VERBOSE("dynamicLatencyAdjustment_MessageHandler %d", id);

    switch(id)
    {
        case DYNAMIC_LATENCY_ADJUSTEMENT_INTERNAL_UPDATE_LATENCY:
        {   
            if(ctx.active)
            {
                dynamicLatencyAdjustment_UpdateLatency();
            }
        }
        break;

        case MIRROR_PROFILE_A2DP_STREAM_ACTIVE_IND:
        {
            DynamicLatencyAdjustment_Start();
        }
        break;

        case MIRROR_PROFILE_A2DP_STREAM_INACTIVE_IND:
        {
            DynamicLatencyAdjustment_Stop();
        }
        break;

        default:
        break;
    }
}

static bool dynamicLatencyAdjustment_CanStart(void)
{
    return  DynamicLatencyAdjustment_IsEnabled() &&
        BtDevice_IsMyAddressPrimary() &&
        MirrorProfile_IsA2dpActive();
}

static void dynamicLatencyAdjustment_UpdateLatency(void)
{
    uint16 adjusted_latency = dynamicLatencyAdjustment_GetAdjustedLatency();
    dynamicLatencyAdjustment_ApplyLatency(adjusted_latency);
    MessageSendLater((Task)&task_data, DYNAMIC_LATENCY_ADJUSTEMENT_INTERNAL_UPDATE_LATENCY, NULL, DYNAMIC_LATENCY_ADJUSTMENT_UPDATE_PERIOD_MS);
}

static uint16 dynamicLatencyAdjustment_GetAdjustedLatency(void)
{
    uint16 adjusted_latency = LatencyConfig_GetTwsLatency();

#ifdef INCLUDE_HDMA_LINK_QUALITY_EVENT
    uint16 lq = Hdma_getLowestLinkQuality();

    if(ctx.fake_lq == 0)
    {
        lq = Hdma_getLowestLinkQuality();
    }
    else
    {
        lq = ctx.fake_lq;
    }


    if(lq != 0xffff)
    {
        DEBUG_LOG_VERBOSE("dynamicLatencyAdjustment_GetAdjustedLatency lowest link quality %d",lq);

        if(lq < LINK_QUALITY_AVERAGE && lq > LINK_QUALITY_LOW)
        {
            adjusted_latency += 100;
        }
        else if (lq <= LINK_QUALITY_LOW)
        {
            adjusted_latency += 200;
        }
        else
        {
            /* Nothing */
        }
    }
#endif

    return adjusted_latency;
}

uint16 DynamicLatencyAdjustment_Marshal(uint8 *buf, uint16 length)
{
    uint16 written = 0;
    size_t size = sizeof(dynamic_latency_adjustment_ctx_t);
    if (length >= size)
    {
        memcpy(buf, &ctx, size);
        written = size;
    }
    return written;
}

uint16 DynamicLatencyAdjustment_Unmarshal(const uint8 *buf, uint16 length)
{
    uint16 read = 0;
    size_t size = sizeof(dynamic_latency_adjustment_ctx_t);
    if (length >= size)
    {
        memcpy(&ctx, buf, size);
        read = size;
    }
    return read;
}

void DynamicLatencyAdjustment_Commit(bool is_primary)
{
    DEBUG_LOG_VERBOSE("DynamicLatencyAdjustment_Commit is_primary %d", is_primary);

    if(is_primary)
    {
        if(ctx.active && dynamicLatencyAdjustment_CanStart())
        {
            DEBUG_LOG_VERBOSE("DynamicLatencyAdjustment_Commit schedule update");
            MessageSendLater((Task)&task_data, DYNAMIC_LATENCY_ADJUSTEMENT_INTERNAL_UPDATE_LATENCY, NULL, DYNAMIC_LATENCY_ADJUSTMENT_UPDATE_PERIOD_MS);
        }
    }
    else
    {
        DynamicLatencyAdjustment_Stop();
    }
}

static void dynamicLatencyAdjustment_SetLatency(uint16 latency_ms)
{
    uint32_t latency_us = (uint32_t)latency_ms * 1000;
    audio_driver_feature_op_uid_t change_latency_op = { .feature=FEATURE_AUDIO_SOURCE, .operation=SET_LATENCY };
    key_value_list_t kv_list = KeyValueList_Create();

    DEBUG_LOG_ALWAYS("dynamicLatencyAdjustment_SetLatency to %u us", latency_us);
    KeyValueList_Add(kv_list, AS_PARAM_SOURCE_TTP_LATENCY_MS, &latency_us, sizeof(latency_us));
    AudioDriver_ConfigureFeature(change_latency_op, kv_list);

    KeyValueList_Destroy(&kv_list);
}

static void dynamicLatencyAdjustment_ApplyLatency(uint16 adjusted_latency)
{
    if(adjusted_latency != ctx.dynamic_latency)
    {
        DEBUG_LOG_VERBOSE("dynamicLatencyAdjustment_ApplyLatency adjusted_latency %d ms", adjusted_latency);
        ctx.dynamic_latency = adjusted_latency;
        dynamicLatencyAdjustment_SetLatency(ctx.dynamic_latency);
    }
}

